#include "zf_common_headfile.h"

uint8_t iptsr[POINTS_MAX_LEN];//最长白列向右扫弦线
uint8_t iptsr_num;
uint8_t iptsL[POINTS_MAX_LEN];//最长白列向左扫弦线
uint8_t iptsL_num;
uint8_t  rpts[POINTS_MAX_LEN];
uint8_t rpts_num;
unsigned char   RowLose[POINTS_MAX_LEN];//丢线数组 1——不丢线 2——只丢右 3——只丢左 4——丢线

/***************************************************************
简化大津法  1.01ms  计算量大时 1.5ms    计算量小时0.7ms

* 函数名称：uint8_t adapt_otsuThreshold(uint8_t Image[ROW][COL], uint16 col, uint16 row)
* 功能说明：获取图像的灰度信息 取最佳阈值
* 参数说明：
* 函数返回：void uint8_t threshold
* 修改时间：2018年3月7日
* 备 注：
***************************************************************/
unsigned char adapt_otsuThreshold(uint8_t *image,uint16_t row_num,unsigned char *threshold)   //注意计算阈值的一定要是原图像
{
       #define GrayScale 256
       int pixelCount[GrayScale];
       float pixelPro[GrayScale];
       int i, j, pixelSum = COL * (row_num)/4;
       //uint8_t threshold = 0;
       uint8_t * data = image;  //指向像素数据的指针
       for (i = 0; i < GrayScale; i++)
       {
          pixelCount[i] = 0;
          pixelPro[i] = 0;
       }

       uint32_t gray_sum=0;
       //统计灰度级中每个像素在整幅图像中的个数
       for (i = ROW_Start; i >=ROW_Start-row_num; i-=2) // 119 117 119-2=117
       {
          for (j = 0; j < COL; j+=2)
          {
             pixelCount[(int)data[i * COL + j]]++;  //将当前的点的像素值作为计数数组的下标
             gray_sum+=(int)data[i * COL + j];       //灰度值总和
          }
       }

       //计算每个像素值的点在整幅图像中的比例

       for (i = 0; i < GrayScale; i++)
       {
          pixelPro[i] = (float)pixelCount[i] / pixelSum;

       }

       //遍历灰度级[0,255]
       float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;



       w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
       for (j = 0; j < GrayScale; j++)
       {

          w0 += pixelPro[j];  //背景部分每个灰度值的像素点所占比例之和   即背景部分的比例
          u0tmp += j * pixelPro[j];  //背景部分 每个灰度值的点的比例 *灰度值

          w1=1-w0;
          u1tmp=gray_sum/pixelSum-u0tmp;

          u0 = u0tmp / w0;              //背景平均灰度
          u1 = u1tmp / w1;              //前景平均灰度
          u = u0tmp + u1tmp;            //全局平均灰度
          deltaTmp = w0 * pow((u0 - u), 2) + w1 * pow((u1 - u), 2);
          if (deltaTmp > deltaMax)
          {
             deltaMax = deltaTmp;
             *threshold = j;
          }
          if (deltaTmp < deltaMax)
          {
             *threshold+=0;
             break;
          }
       }
       return *threshold;
}
/**
 * 传入最长白列,
 * search_line(Gray_Image[0],ROW-1);
 */
void search_line(const uint8_t *img,uint8_t begin_y,uint8_t end_y)
{
uint8_t Row,rx=Image_DataStruct.begin_x;//0代表成功
uint8_t lx=Image_DataStruct.begin_x;
uint8_t rflag; //1代表下次按照边缘追踪方式 搜线
uint8_t lflag;
iptsL_num=0;
iptsr_num=0;
unsigned char tempr_c0;//右边线 列比较 当前存储像素值
unsigned char tempr_c1;//右边线 列比较 比较存储像素值
unsigned char tempr_r0;//右边线 行比较 当前存储像素值
unsigned char tempr_r1;//右边线 行比较 比较存储像素值

unsigned char templ_c0;//左边线 列比较 当前存储像素值
unsigned char templ_c1;//左边线 列比较 比较存储像素值
unsigned char templ_r0;//左边线 行比较 当前存储像素值
unsigned char templ_r1;//左边线 行比较 比较存储像素值
if(end_y<COL_STEP){end_y=COL_STEP;} //限幅保护

for (Row = begin_y; Row >= end_y; Row--)//对应坐标轴系y
{
   //左边线
   for (; lx > 0;lx--)//对应坐标轴系x
   {
    templ_c0 = img[Row*COL+lx];
    templ_c1 = img[Row*COL+lx-COL_STEP];
      if(lx-COL_STEP<=COL_Start) {iptsL[Row]=0;break;}
      if(Row<=ROW/2)
      {
        if(templ_c1<=Image_DataStruct.white_min_point) //图像后半部分
        {
                iptsL[Row]=lx;
                break;
        }
        if( Diff_Ratio(templ_c0 , templ_c1) > Image_DataStruct.Diff_Ratio)
        {
            iptsL[Row]=lx;
            break;
        }
      }else{
           if( Diff_Ratio(templ_c0 , templ_c1) > Image_DataStruct.Diff_Ratio)
            {
                iptsL[Row]=lx;
                break;
            }
      }

   }

templ_r0 = img[Row*COL + (lx+COL_STEP) ];
templ_r1 = img[(Row-COL_STEP)*COL + (lx+COL_STEP)];
   if(iptsL[Row]==COL_Start)
   {
        if( Diff_Ratio(templ_r0, templ_r1)>=Image_DataStruct.Diff_Ratio
            || templ_r1 <= Image_DataStruct.Refer_Threshold )
        {
            lx=Image_DataStruct.begin_x;
            lflag=0;
        }
        else
        {
            lx+=SEARCHRANGE;
            lflag=0;
        }
   }
   else
    {
        if(templ_r1 <= Image_DataStruct.Refer_Threshold || lx >=COL_End-SEARCHRANGE)//防止突破右侧边界
        {
            lx=Image_DataStruct.begin_x;
            lflag=1;
        }
        else{
            // lx = (lx>=COL_End-SEARCHRANGE)?Image_DataStruct.begin_x:(lx+SEARCHRANGE);
            lx+=SEARCHRANGE;
            lflag=1;
        }

    }


   //右边线
   for (; rx < COL-1;rx++)//对应坐标轴系x
   {
    tempr_c0 = img[Row*COL+rx];
    tempr_c1 = img[Row*COL+rx+COL_STEP];
      if(rx+COL_STEP>=COL_End-1) {iptsr[Row]=COL_End; break;}
      
      if(Row<=ROW/2)
      {
        if(tempr_c1<=Image_DataStruct.white_min_point)
        {
            iptsr[Row]=rx;
            break;
        }
        if( Diff_Ratio( tempr_c0,tempr_c1 ) >= Image_DataStruct.Diff_Ratio)
        {
            iptsr[Row]=rx;
            break;
        }
      }
      else
      {
        if( Diff_Ratio( tempr_c0,tempr_c1 ) >= Image_DataStruct.Diff_Ratio)
        {
            iptsr[Row]=rx;
            break;
        }
      }

   }
tempr_r0 = img[Row*COL + (rx-COL_STEP) ];
tempr_r1 = img[(Row-COL_STEP)*COL + (rx-COL_STEP)];
   if(iptsr[Row]==COL_End)
   {
        if( Diff_Ratio(tempr_r0, tempr_r1)>=Image_DataStruct.Diff_Ratio
            || /* img[(Row-3)*COL+rx-COL_STEP] */tempr_r1 <= Image_DataStruct.Refer_Threshold )
        {
            rx=Image_DataStruct.begin_x;
            rflag=0;
        }
        else
        {
            rx-=SEARCHRANGE;
            // rx=rx<SEARCHRANGE?SEARCHRANGE:rx;
            rflag=0;
        }
   }
   else
    {
        if(/* img[(Row-3)*COL+rx] */tempr_r1 <= Image_DataStruct.Refer_Threshold || rx <=COL_Start+SEARCHRANGE)
        {
            rx=Image_DataStruct.begin_x;
            rflag=1;
        }
        else{
            // rx = (rx<=COL_Start+SEARCHRANGE)?Image_DataStruct.begin_x:(rx-SEARCHRANGE);//防止突破左侧边界
            rx-=SEARCHRANGE;
            // rx=rx<SEARCHRANGE?SEARCHRANGE:rx;
            rflag=1;
        }

    }

   if(lflag==1 && rflag == 1){RowLose[Row]=1;}
   else if(lflag==1 && rflag == 0){RowLose[Row]=2;} //丢右
   else if(lflag==0 && rflag == 1){RowLose[Row]=3;} //丢左
   else if(lflag==0 && rflag == 0){RowLose[Row]=4;} //双丢


}

}


// void search_line(const uint8_t *img,uint8_t begin_y,uint8_t end_y)
// {
// uint8_t Row,rx=Image_DataStruct.begin_x;//0代表成功
// uint8_t lx=Image_DataStruct.begin_x;
// uint8_t rflag; //1代表下次按照边缘追踪方式 搜线
// uint8_t lflag;
// iptsL_num=0;
// iptsr_num=0;

// for (Row = begin_y; Row >= end_y; Row--)//对应坐标轴系y
// {
//    //左边线
//    for (; lx > 0;lx--)//对应坐标轴系x
//    {
//       if(lx-COL_STEP<1) {iptsL[Row]=0;break;}
//     //    if(img[Row*COL+lx-COL_STEP] < Image_DataStruct.white_min_point){iptsL[Row]=lx;/*lflag=1;*/break;}
//       if( Diff_Ratio(img[Row*COL+lx] , img[Row*COL+lx-COL_STEP]) > Image_DataStruct.Diff_Ratio)
//       {
//          iptsL[Row]=lx;
//          break;
//       }
//    }
//       if( (lx-COL_STEP<1) || lx >COL-SEARCHRANGE) {lx=Image_DataStruct.begin_x;lflag=0;} //根据上一次的信息,决定这一次是否启用边缘追踪
//       else
//       {
//           lx+=SEARCHRANGE;
//           lx = (lx>COL_End?COL_End-SEARCHRANGE:lx);
//           lflag=1;
//       }

//    //右边线
//    for (; rx < COL-1;rx++)//对应坐标轴系x
//    {
//       if(rx+COL_STEP>=COL_End-1) {iptsr[Row]=COL_End; break;}
//     //    if(img[Row*COL+rx+COL_STEP] < Image_DataStruct.white_min_point){iptsr[Row]=rx;/*rflag=1;*/ break;}
//       if( Diff_Ratio(img[Row*COL+rx] , img[Row*COL+rx+COL_STEP]) >= Image_DataStruct.Diff_Ratio/*  || img[Row*COL+rx]<=Image_DataStruct.white_min_point */)
//       {
//          iptsr[Row]=rx;
//          break;
//       }
//    }


//     //   if((rx+COL_STEP>COL-2) || rx <SEARCHRANGE) {rflag=0; rx=Image_DataStruct.begin_x;} //根据上一次的信息,决定这一次是否启用边缘追踪
//     //   else
//     //   {
//         //   rx-=SEARCHRANGE;
//         //   rx=rx<SEARCHRANGE?SEARCHRANGE:rx;
//         //   rflag=1;
//     //   }

//    if(lflag==1 && rflag == 1){RowLose[Row]=1;}
//    else if(lflag==1 && rflag == 0){RowLose[Row]=2;} //丢右
//    else if(lflag==0 && rflag == 1){RowLose[Row]=3;} //丢左
//    else if(lflag==0 && rflag == 0){RowLose[Row]=4;} //双丢


// }

// }

//void fit_mid_line(void)
//{
//   uint8_t midy,counter=0;
//   rpts_num = min(iptsr_num,iptsL_num);
//
//     for (midy = ROW-1; midy >ROW-rpts_num; midy--)
//     {
//        rpts[counter][0]=(iptsr[midy]+iptsL[midy])/2;
//        rpts[counter][1] = midy;
//        counter++;
//     }
//
//rpts_num = counter-1;
//}

//void get_reference_point(const uint8_t * image,uint8_t *reference_point,uint8_t *white_max_point,uint8_t *white_min_point)
//{
//uint8_t *p = (uint8_t *)&image[(ROW-REFERENCEROW)*COL]; //115*188
//uint16_t temp=0; //保存统计点总数量
//uint32_t temp_sum =0; //保存所有统计点加起来的和 must be uint
//temp = REFERENCEROW*COL; //计算待统计点总数量
//for(uint16_t i=0;i<temp;i++){
//temp_sum += *(p+i);
//}
//*reference_point = (uint8_t)(temp_sum/temp);
//*white_max_point = (uint8_t)func_limit_ab((uint16_t)(*reference_point * WHITEMAXMUL/10),BLACKPOINT,255);
//*white_min_point = (uint8_t)func_limit_ab((uint16_t)(*reference_point * WHITEMINMUL /10),BLACKPOINT,255);
//}

void get_reference_point(const uint8_t * image)
{
uint8_t *p = (uint8_t *)&image[(ROW-REFERENCEROW)*COL]; //55*94
uint16_t temp=0; //保存统计点总数量
uint32_t temp_sum =0; //保存所有统计点加起来的和 must be uint
temp = REFERENCEROW*COL; //计算待统计点总数量
for(uint16_t i=0;i<temp;i++){
temp_sum += *(p+i);
}
Image_DataStruct.Refer_Threshold = (uint8_t)(temp_sum/temp);
Image_DataStruct.white_max_point = (uint8_t)func_limit_ab((uint16_t)(Image_DataStruct.Refer_Threshold * WHITEMAXMUL/10),BLACKPOINT,255);
Image_DataStruct.white_min_point = (uint8_t)func_limit_ab((uint16_t)(Image_DataStruct.Refer_Threshold * WHITEMINMUL /10),BLACKPOINT,255);
}



/**每个几列进行查找黑点,根据参考白点
 * 改良版本逐飞最长白列
 */
unsigned char white_col[COL];
void longest_column(uint8_t * img,uint8_t begin_col,uint8_t end_col)
{
memset(white_col,0,COL);//首先清零
uint8_t lc,lr,temp1,temp2; //lc~COL lr~ROW
int temp3;
Image_DataStruct.lst_rows=0;//document the biggest longest white column 记录最长白列最大值
Image_DataStruct.begin_x=COL/2;
for(lc=begin_col;lc<end_col;lc+=LSC_STEP){ //from column
    for(lr=ROW-1;lr>STOP_ROW;lr-=LSR_STEP){//from ROW
        temp1 = *(img+lr*COL+lc); //获取当前灰度值
        temp2 = *(img+(lr-LSR_STEP)*COL+lc); //获取对比点灰度值

        if((lr-LSR_STEP) <= STOP_ROW){ //触碰边界
            white_col[lc]=ROW -1- lr;
            break;
        }
        if(lr<=25)
        {
                  if (temp1 < Image_DataStruct.Refer_Threshold  || temp1 <= BLACKPOINT){// white_min_point ROW  Refer_Threshold* cols + COL = ROW,COL 小于超级黑点,直接把这一列结束掉 极度拐弯时,前五行90%都是暗点 得到的所有计算的点都没有参考价值了,固定给值黑点这个就得是经验值了
                    white_col[lc]=ROW -1- lr;
                    break;//小于超级黑点,跳出,进行下列
                }
                if(temp2 > Image_DataStruct.white_max_point){continue;} //大于超级白点,直接跳过,进行下一行

                    temp3 = Diff_Ratio(temp1,temp2); //calculate refer 计算对比度

                    if(temp3 > Image_DataStruct.Diff_Ratio){ //如果对比度大于阈值,或者触碰边界
                        white_col[lc]=ROW -1- lr;
                        break;
                    }
        }
        else{
                  if (temp1 < Image_DataStruct.white_min_point  || temp1 <= BLACKPOINT){// white_min_point ROW  Refer_Threshold* cols + COL = ROW,COL 小于超级黑点,直接把这一列结束掉 极度拐弯时,前五行90%都是暗点 得到的所有计算的点都没有参考价值了,固定给值黑点这个就得是经验值了
                    white_col[lc]=ROW -1- lr;
                    break;//小于超级黑点,跳出,进行下列
                }
                if(temp2 > Image_DataStruct.white_max_point){continue;} //大于超级白点,直接跳过,进行下一行

                    temp3 = Diff_Ratio(temp1,temp2); //calculate refer 计算对比度

                    if(temp3 > Image_DataStruct.Diff_Ratio){ //如果对比度大于阈值,或者触碰边界
                        white_col[lc]=ROW -1- lr;
                        break;
                    }
        }

    }
if (white_col[lc] > Image_DataStruct.lst_rows){Image_DataStruct.lst_rows = white_col[lc];Image_DataStruct.begin_x=lc;}
}

if( Image_DataStruct.lst_rows==0){System_DataStruct.stop_flag=-1;}
else{System_DataStruct.stop_flag=0;}//停车
}



unsigned char white_row[ROW];
void longest_row(const unsigned char *img,unsigned char begin_col,unsigned char end_row,unsigned char way)
{
memset(white_row,0,ROW);//首先清零
uint8_t lc,lr,temp1,temp2; //lc~COL lr~ROW
int temp3;
Image_DataStruct.lst_cols=0;//document the biggest longest white column 记录最长白行最大值
Image_DataStruct.longest_y=0;

switch (way)
{
case 0: //左0   
    for(lr=ROW_Start;lr>end_row;lr-=LSR_STEP){ //from ROW
    for(lc=begin_col;lc>COL_Start;lc-=LSR_STEP){//from column
        temp1 = *(img+lr*COL+lc); //获取当前灰度值
        temp2 = *(img+lr*COL+lc-LSC_STEP); //获取对比点灰度值

        if((lc-LSC_STEP) <= COL_Start){ //触碰边界
            white_row[lr]=begin_col;
            break;
        }
      if (temp1 < Image_DataStruct.white_min_point){
        white_row[lr]=begin_col-lc;
        break;
      }
      if(temp2 > Image_DataStruct.white_max_point){continue;} //大于超级白点,直接跳过,进行下一列

        temp3 = Diff_Ratio(temp1,temp2); //calculate refer 计算对比度

        if(temp3 > Image_DataStruct.Diff_Ratio){ 
            white_row[lr]=begin_col-lc;
            break;
        }
    }
    if (white_row[lr] > Image_DataStruct.lst_cols){Image_DataStruct.lst_cols = white_row[lr];Image_DataStruct.longest_y=lr;}
    }
    break;
case 1: //右1
    for(lr=ROW_Start;lr>end_row;lr-=LSR_STEP){ //from ROW
    for(lc=begin_col;lc<COL_End;lc+=LSR_STEP){//from column
        temp1 = *(img+lr*COL+lc); //获取当前灰度值
        temp2 = *(img+lr*COL+lc+LSC_STEP); //获取对比点灰度值
        // printf("1");
        if((lc+LSC_STEP) >= COL_End){ //触碰边界
            white_row[lr]=COL_End-begin_col;
            break;
        }
      if (temp1 <= Image_DataStruct.Refer_Threshold/* white_min_point */){
        white_row[lr]=lc-begin_col;
        break;//小于超级黑点,跳出,进行下行
      }
      if(temp2 > Image_DataStruct.white_max_point){continue;} //大于超级白点,直接跳过,进行下一列

        temp3 = Diff_Ratio(temp1,temp2); //calculate refer 计算对比度

        if(temp3 > Image_DataStruct.Diff_Ratio){ 
            white_row[lr]=lc-begin_col;
            break;
        }
    }
    if (white_row[lr] >= Image_DataStruct.lst_cols){Image_DataStruct.lst_cols = white_row[lr];Image_DataStruct.longest_y=lr;}
    }
        // printf("1");

break;
default:
    break;
}

}

//uint8_t white_row[COL];
//void longest_column(uint8_t * img,uint8_t row,uint8_t col,uint8_t * begin_x,uint8_t * lst_rows)
//{
//memset(white_row,0,COL);//首先清零
//uint8_t lc,lr,temp1,temp2; //lc~col lr~row
//int temp3;
//*lst_rows=0;//document the biggest longest white column 记录最长白列最大值
//*begin_x=COL/2;
//for(lc=LSC_STEP;lc<col-LSC_STEP;lc+=LSC_STEP){ //from column
//    for(lr=row-1;lr>STOP_ROW;lr-=LSR_STEP){//from row
//        temp1 = *(img+lr*col+lc); //获取当前灰度值
//        temp2 = *(img+(lr-LSR_STEP)*col+lc); //获取对比点灰度值
//
//        if(lr <= STOP_ROW){ //触碰边界
//            white_row[lc]=ROW -1- lr;
//            break;
//        }
//      if (temp1 < Image_DataStruct.white_min_point || temp1 < BLACKPOINT){//row * cols + col = row,col 小于超级黑点,直接把这一列结束掉 极度拐弯时,前五行90%都是暗点 得到的所有计算的点都没有参考价值了,固定给值黑点这个就得是经验值了
//        white_row[lc]=ROW -1- lr;
//        break;//小于超级黑点,跳出,进行下列
//      }
//      if(temp2 > Image_DataStruct.white_max_point){continue;} //大于超级白点,直接跳过,进行下一行
//
//        temp3 = Diff_Ratio(temp1,temp2); //calculate refer 计算对比度
//
//        if(temp3 > Image_DataStruct.Diff_Ratio){ //如果对比度大于阈值,或者触碰边界
//            white_row[lc]=ROW -1- lr;
//            break;
//        }
//    }
//if (white_row[lc] > *lst_rows){*lst_rows = white_row[lc];*begin_x=lc;}
//}
//
//if( *lst_rows==0){System_DataStruct.stop_flag=-1;}
//else{System_DataStruct.stop_flag=0;}//停车
//}




unsigned char ipts0[POINTS_MAX_LEN][2] ;//左
unsigned char ipts1[POINTS_MAX_LEN][2] ;//右
unsigned char ipts0_num;
unsigned char ipts1_num;
unsigned char block_size =7;
unsigned char clip_value =3;

/* 前进方向定义：
 *   0
 * 3   1
 *   2
 */
const int dir_front[4][2] = {{0,  -1},
                            {1,  0},
                            {0,  1},
                            {-1, 0}};
const int dir_frontleft[4][2] = {{-1, -1},
                                {1,  -1},
                                {1,  1},
                                {-1, 1}};
const int dir_frontright[4][2] = {{1,  -1},
                                 {1,  1},
                                 {-1, 1},
                                 {-1, -1}};

//左手迷宫寻线
/**
*@param7:*num=119，0~119(POINTS_MAX_LEN=120的情况下)
*@author:C先生
*@date:
*/
void findline_lefthand_adaptive(unsigned char *img, unsigned char block_size, unsigned char clip_value, unsigned char x, unsigned char y, unsigned char pts[][2], unsigned char *num)
{

    int half = block_size / 2;
    uint8_t step = 0, dir = 0, turn = 0;
    while (step < *num && 0 < x && x < COL_End - half  && half < y && y < ROW_Start - half  && turn < 4)
    {
        int local_thres = 0;
        for (int dy = -half; dy <= half; dy++)
        {
            for (int dx = -half; dx <= half; dx++)
             {
                if ((x + dx)<0 || y+dy>COL_End)
                    continue;
                else
                      local_thres += TC(img, x + dx, y + dy);
            }
        }
        local_thres /= block_size * block_size;
        local_thres -= clip_value;

        int front_value = TC(img, x + dir_front[dir][0], y + dir_front[dir][1]);
        int frontleft_value = TC(img, x + dir_frontleft[dir][0], y + dir_frontleft[dir][1]);
        if (front_value < local_thres)
        {
            dir = (dir + 1) % 4;
            turn++;
        } else if (frontleft_value < local_thres)
        {
            x += dir_front[dir][0];
            y += dir_front[dir][1];
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
        } else
         {
            x += dir_frontleft[dir][0];
            y += dir_frontleft[dir][1];
            dir = (dir + 3) % 4;
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
        }
    }
    *num = step;//点数 120
}

/// @brief 右手迷宫巡线,返回的ipts1_num是点的数量,如果要靠ipts1_num查询坐标,需要减一
/// @param img 
/// @param block_size 
/// @param clip_value 
/// @param x 
/// @param y 
/// @param pts 
/// @param num 
void findline_righthand_adaptive(unsigned char *img, unsigned char block_size, unsigned char clip_value, unsigned char x, unsigned char y, unsigned char pts[][2], unsigned char *num)
{

    int half = block_size / 2;
    uint8_t step = 0, dir = 0, turn = 0;//
    while (step < *num && half < x && x < COL_End-half && half < y && y < ROW_Start-half && turn < 4)
      {
          int local_thres = 0;
          for (int dy = -half; dy <= half; dy++)
          {
              for (int dx = -half; dx <= half; dx++)
              {
                  if(x+dx>COL_End || y+dy>COL_End )
                  continue;
                  else
                  local_thres += TC(img, x + dx, y + dy);
              }
          }
          local_thres /= block_size * block_size;
          local_thres -= clip_value;

          int front_value = TC(img, x + dir_front[dir][0], y + dir_front[dir][1]);
          int frontright_value = TC(img, x + dir_frontright[dir][0], y + dir_frontright[dir][1]);
          if (front_value < local_thres)//如果前方为黑色
          {
              dir = (dir + 3) % 4; //转头向左
              turn++;
          } else if (frontright_value < local_thres) //如果前方为白色 且 前右方为黑色,turn=0,dir保持不边
          {
              x += dir_front[dir][0];
              y += dir_front[dir][1];
              pts[step][0] = x;
              pts[step][1] = y;
              step++;
              turn = 0;
          }
          else//如果前方为白色  前右方也为白色,向前右方跳变
           {
              x += dir_frontright[dir][0];
              y += dir_frontright[dir][1];
              dir = (dir + 1) % 4;
              pts[step][0] = x;
              pts[step][1] = y;
              step++;
              turn = 0;
          }
      }
      *num = step;
}

void find_line_r(unsigned char y1,unsigned char num)
{
unsigned char x1,limit_x;
ipts1_num = num;
x1 = iptsr[y1];
limit_x = (iptsr[y1] + SEARCHRANGE)>COL_End?COL_End:(iptsr[y1] + SEARCHRANGE);

for(;x1 <= limit_x; x1++)
{
    if(Diff_Ratio(TC(Gray_Image[0], x1, y1),TC(Gray_Image[0],x1 + 2, y1))> Image_DataStruct.Diff_Ratio) {break;}
}
findline_righthand_adaptive(Gray_Image[0], block_size, clip_value,x1, y1, ipts1, &ipts1_num);

}


void find_line_l(unsigned char y0,unsigned char num)
{
unsigned char x0,limit_x;
ipts0_num = num;
x0 = iptsL[y0];
limit_x = (iptsL[y0]-SEARCHRANGE)<1?1:(iptsL[y0]-SEARCHRANGE);

for (; x0 > limit_x;x0--)
if (Diff_Ratio( TC(Gray_Image[0], x0,y0),TC(Gray_Image[0],x0-2, y0) ) > Image_DataStruct.Diff_Ratio) break;
findline_lefthand_adaptive(Gray_Image[0], block_size, clip_value, x0, y0, ipts0, &ipts0_num);

}

